#include "slamwidget.h"

#include <QPainter>
#include <QWheelEvent>
#include <QSlider>
#include <QVBoxLayout>
#include <QScrollBar>
#include <QPointer>
#include <QTimer>
#include <QElapsedTimer>
#include <QCoreApplication>

#include <models/robotlist.h>
#include<math.h>

SlamWidget::SlamWidget(QWidget *parent)
    : QWidget{parent}
{
    setAttribute(Qt::WA_StyledBackground, true);  // 允许 Widget 使用样式表设置背景
    setStyleSheet(" background-color: black;border: 0px; ");

    m_pCurRobot = RobotList::getInstance()->getCurRobot();
    m_pScale = new ScaleLabel(this);
    m_pScale->setFixedHeight(35*g_xScale);
    m_pCtrlWidget = new QWidget(this);
    m_pCtrlWidget->setStyleSheet("background-color:transparent");
    m_pRangStart = NULL;
    m_pRangEnd = NULL;
    m_pRangLab = NULL;
    m_pDest = NULL;
    m_goDestBtn = NULL;
    m_bDragging = false;
    m_bDragRangStart= false;
    m_bDragRangEnd= false;

    //导航按钮
    m_pNavigateBtn = new TouchButton(m_pCtrlWidget);
    m_pNavigateBtn->setText("");
    m_pNavigateBtn->setFixedSize(60*g_xScale,60*g_xScale);
    m_pNavigateBtn->setCheckable(true);
    m_pNavigateBtn->setStyleSheet("QPushButton { border-image:url(:/images/slam_navigate.png); } QPushButton:checked {  border-image:url(:/images/slam_navigate_select.png); }");
    connect(m_pNavigateBtn,&QPushButton::toggled,this,[this](bool bChecked){
        if(bChecked)
        {
            if(m_pSetStartBtn->isChecked())
                m_pSetStartBtn->setChecked(false);
            if(m_pRangBtn->isChecked())
                m_pRangBtn->setChecked(false);
            m_Mode = MAP_NAVIGATE;
            m_MouseTimer.start(30);
        }
        else
        {
            if(m_pDest)
            {
                m_pDest->deleteLater();
                m_pDest = NULL;
            }
            if(m_goDestBtn)
            {
                m_goDestBtn->deleteLater();
                m_goDestBtn = NULL;
            }
            if(m_Mode == MAP_NAVIGATING && !m_bToDest)
            {
                if(m_pCurRobot)
                {
                    m_pCurRobot->stopNavigatione();
                }
            }
            //m_Mode = MAP_NORMAL;
            setShowMax(m_bShowMax);
            m_MouseTimer.stop();
            update();
        }
    });

    //测距按钮
    m_pRangBtn = new TouchButton(m_pCtrlWidget);
    m_pRangBtn->setText("");
    m_pRangBtn->setFixedSize(60*g_xScale,60*g_xScale);
    m_pRangBtn->setCheckable(true);
    m_pRangBtn->setStyleSheet("QPushButton { border-image:url(:/images/slam_rang.png); } QPushButton:checked {  border-image:url(:/images/slam_rang_select.png); }");
   connect(m_pRangBtn,&QPushButton::toggled,this,[this](bool bChecked){
        if(bChecked)
        {
            if(m_pNavigateBtn->isChecked())
                m_pNavigateBtn->setChecked(false);
            if(m_pSetStartBtn->isChecked())
                m_pSetStartBtn->setChecked(false);
            m_Mode = MAP_RANG;
            m_MouseTimer.start(30);
        }
        else
        {
            if(m_pRangStart)
            {
                m_pRangStart->deleteLater();
                m_pRangStart = NULL;
            }
            if(m_pRangEnd)
            {
                m_pRangEnd->deleteLater();
                m_pRangEnd = NULL;
            }
            if(m_pRangLab)
            {
                m_pRangLab->deleteLater();
                m_pRangLab = NULL;
            }
            m_bDragging = false;
            m_bDragRangStart= false;
            m_bDragRangEnd= false;
            m_Mode = MAP_NORMAL;
            m_MouseTimer.stop();
            setMouseTracking(false);
            update();
        }
    });

    //设置机器人起始位置按钮
    m_pSetStartBtn = new TouchButton(m_pCtrlWidget);
    m_pSetStartBtn->setText("");
    m_pSetStartBtn->setFixedSize(60*g_xScale,60*g_xScale);
    m_pSetStartBtn->setCheckable(true);
    m_pSetStartBtn->setStyleSheet("QPushButton { border-image:url(:/images/slam_setstart.png); } QPushButton:checked {  border-image:url(:/images/slam_setstart_select.png); }");

    connect(m_pSetStartBtn,&QPushButton::toggled,this,[this](bool bChecked){
        if(bChecked)
        {
            if(m_pNavigateBtn->isChecked())
                m_pNavigateBtn->setChecked(false);
            if(m_pRangBtn->isChecked())
                m_pRangBtn->setChecked(false);
            m_Mode = MAP_SETROBOT;
            m_MouseTimer.start(30);
        }
        else
        {
            m_Mode = MAP_NORMAL;
            m_MouseTimer.stop();
        }
    });

    //机器人居中按钮
    m_pCenterBtn = new TouchButton(m_pCtrlWidget);
    m_pCenterBtn->setText("");
    m_pCenterBtn->setFixedSize(60*g_xScale,60*g_xScale);
    m_pCenterBtn->setCheckable(true);
    m_pCenterBtn->setStyleSheet("QPushButton { border-image:url(:/images/slam_center.png); } QPushButton:checked {  border-image:url(:/images/slam_center_select.png); }");
    connect(m_pCenterBtn,&QPushButton::toggled,this,[&](bool bChecked){
        if(bChecked&&m_pCurRobot)
        {
            setOffSet(width()/2 - (m_pCurRobot->mapRobot().x() + m_pCurRobot->mapOrigin().x()) / m_pCurRobot->mapRes() * m_scaleFactor,
                      height()/2 + (m_pCurRobot->mapRobot().y() + m_pCurRobot->mapOrigin().y()) / m_pCurRobot->mapRes() * m_scaleFactor);
            m_inertiaTimer.stop();
        }
    });
    QWidget * zoomWidget = new QWidget(m_pCtrlWidget);
    zoomWidget->setFixedSize(60*g_xScale,300*g_xScale);
    zoomWidget->setStyleSheet(QString("background-color: #2E2F31;  \
         border: none;  \
         border-radius: %1px; ").arg(int(8*g_xScale)));

    //放大按钮
    m_pZoomInBtn = new TouchButton(zoomWidget);
    m_pZoomInBtn->setText("");
    connect(m_pZoomInBtn,&QPushButton::clicked,this,[this](){
        setScaleFactor(rect().center(),m_scaleFactor*1.1);
    });
    m_pZoomInBtn->setFixedSize(60*g_xScale,60*g_xScale);
    m_pZoomInBtn->setStyleSheet("QPushButton { border-image:url(:/images/slam_zoomIn.png); } QPushButton:pressed {  border-image:url(:/images/slam_zoomIn_pressed.png); }");
    //缩小按钮
    m_pZoomOutBtn = new TouchButton(zoomWidget);
    m_pZoomOutBtn->setText("");
    connect(m_pZoomOutBtn,&QPushButton::clicked,this,[this](){
        setScaleFactor(rect().center(),m_scaleFactor*0.9);
    });
    m_pZoomOutBtn->setFixedSize(60*g_xScale,60*g_xScale);
    m_pZoomOutBtn->setStyleSheet("QPushButton { border-image:url(:/images/slam_zoomOut.png); } QPushButton:pressed {  border-image:url(:/images/slam_zoomOut_pressed.png); }");
    //缩放滑块
    m_pZoomSlider = new QSlider(zoomWidget);
    m_pZoomSlider->setFixedWidth(30*g_xScale);
    m_pZoomSlider->setRange(30,300);

    connect(m_pZoomSlider, &QSlider::valueChanged,this,[this](int value){
        setScaleFactor(rect().center(), value * 1.0 / 100);
    });
    QVBoxLayout *zoomLayout = new QVBoxLayout(zoomWidget);
    zoomLayout->setMargin(0);
    zoomLayout->setSpacing(0);
    zoomLayout->setAlignment(Qt::AlignHCenter); // Add this line to center align the slider

    zoomLayout->addWidget(m_pZoomInBtn);
    zoomLayout->addWidget(m_pZoomSlider, 0, Qt::AlignHCenter); // Add alignment to the slider
    zoomLayout->addWidget(m_pZoomOutBtn);

    QVBoxLayout *ctrlLayout = new QVBoxLayout(m_pCtrlWidget);
    ctrlLayout->setMargin(0);
    ctrlLayout->setSpacing(20*g_yScale);

    ctrlLayout->addStretch();
    ctrlLayout->addWidget(m_pNavigateBtn);
    ctrlLayout->addWidget(m_pRangBtn);
    ctrlLayout->addWidget(m_pSetStartBtn);
    ctrlLayout->addWidget(m_pCenterBtn);
    ctrlLayout->addWidget(zoomWidget);

    //机器人图标
    m_pRobot = new QLabel(this);
    m_pRobot->setFixedSize(130*g_xScale,130*g_xScale);
    m_pRobot->setStyleSheet(" background-color:transparent;");
    m_pRobot->setAlignment(Qt::AlignCenter);
    m_Mode = MAP_ERROR;

    onMapChanged();
    bindRobot();
    connect(RobotList::getInstance(),&RobotList::curRobotChanged,this,[this](){
        if(m_pCurRobot)
        {
            disconnect(m_pCurRobot,NULL,this,NULL);
        }
        m_pCurRobot = RobotList::getInstance()->getCurRobot();
        bindRobot();
    });

    connect(&m_inertiaTimer, &QTimer::timeout, this, &SlamWidget::onInertiaMove);
    connect(&m_MouseTimer, &QTimer::timeout, this, &SlamWidget::onMouseTimer);
    this->setAttribute(Qt::WA_AcceptTouchEvents);
    m_pCtrlWidget->raise();
    m_pCenterBtn->setChecked(true);
}

void SlamWidget::setShowMax(bool newShowMax)
{
    // if(m_Mode != MAP_ERROR)
    // {
    m_bShowMax = newShowMax;
    if(newShowMax)
    {
        if(m_Mode != MAP_ERROR && m_Mode != MAP_NAVIGATING)
            m_Mode = MAP_NORMAL;
        m_pCtrlWidget->show();
    }
    else
    {
        m_pCtrlWidget->hide();
        //m_pNavigateBtn->setChecked(false);
        m_pSetStartBtn->setChecked(false);
        m_pRangBtn->setChecked(false);
        m_pCenterBtn->setChecked(true);
        if(m_pCurRobot)
            setOffSet(width()/2 - (m_pCurRobot->mapRobot().x() + m_pCurRobot->mapOrigin().x()) / m_pCurRobot->mapRes() * m_scaleFactor,
                    height()/2 + (m_pCurRobot->mapRobot().y() + m_pCurRobot->mapOrigin().y()) / m_pCurRobot->mapRes() * m_scaleFactor);
        if(m_Mode != MAP_ERROR && m_Mode != MAP_NAVIGATING)
            m_Mode = MAP_MINSIZE;
    }
    update();
    //}
}

void SlamWidget::onMapChanged()
{
    if(m_pCurRobot)
    {
        m_pixMap = QPixmap::fromImage(*m_pCurRobot->pImgMap());
        setScaleFactor(rect().center(),1.0);
        m_pCenterBtn->setChecked(true);
        m_bDragging = false;
        m_bDragRangStart= false;
        m_bDragRangEnd= false;
        setOffSet((width() - m_pixMap.width())/2,(height() - m_pixMap.height())/2);
        m_Mode = MAP_NORMAL;
        setShowMax(m_bShowMax);
        update();
        ///默认机器人在原点位置
        setRobotPosition(QPoint(0,0));
        setRobotRotation(0);
    }

}

void SlamWidget::mousePressEvent(QMouseEvent *event)
{
    if(event->button() == Qt::LeftButton){
        if(MAP_MINSIZE != m_Mode)
            m_pCenterBtn->setChecked(false);
        switch(m_Mode)
        {
        case MAP_ERROR:
        case MAP_MINSIZE:
            break;
        case MAP_NORMAL:
            break;
        case MAP_NAVIGATING:
            break;
        case MAP_NAVIGATE:
        {
            if(!m_pDest)
            {
                m_pDest = new QLabel(this);
                m_pDest->setStyleSheet(" background-color:transparent;");
                m_pDest->setFixedSize(130*g_xScale,130*g_xScale);
                m_pDest->setPixmap(QPixmap(":/images/map_dest.png").scaled(m_pDest->width(), m_pDest->height(), Qt::KeepAspectRatio, Qt::SmoothTransformation));
                m_pDest->setAlignment(Qt::AlignCenter);
                m_pDest->show();
                m_fDestAngle = 0;
                m_pCtrlWidget->raise();
            }
            m_pDest->move(event->pos().x() - m_pRobot->width()/2,event->pos().y() - m_pRobot->height()/2);
            m_ptDest = ui2real(event->pos());
            if(m_goDestBtn)
                m_goDestBtn->hide();
            update();
        }
        break;
        case MAP_SETROBOT:
        {
            m_pRobot->move(event->pos().x() - m_pRobot->width()/2,event->pos().y() - m_pRobot->height()/2);
        }
        break;
        case MAP_RANG:
            if(m_pRangStart == NULL || m_pRangLab == NULL)
            {
                m_pRangStart = new QLabel(this);
                m_pRangStart->setStyleSheet(QString("border: 4px solid red; background-color: transparent; border-radius: %1px;").arg(int(20*g_xScale)));
                m_pRangStart->setFixedSize(40*g_xScale, 40*g_xScale);
                m_pRangStart->move(event->pos().x()-m_pRangStart->width()/2,event->pos().y()-m_pRangStart->height()/2);
                m_pRangStart->show();
                m_pRangLab = new QLabel(this);
                m_pRangLab->setFixedHeight(30*g_xScale);
                m_pRangLab->setStyleSheet(QString("background-color:black;color: white; text-align: center; font:%1px \"微软雅黑\"").arg(int(18*g_xScale)));
                m_pRangLab->show();
                m_ptRangStart = ui2real(event->pos());
                updateRang(event->pos(),event->pos());
                setMouseTracking(true);
                m_pCtrlWidget->raise();
            }
            else if(m_pRangEnd == NULL)
            {
                m_pRangEnd = new QLabel(this);
                m_pRangEnd->setStyleSheet(QString("border: 4px solid red; background-color: transparent; border-radius: %1px;").arg(int(20*g_xScale)));
                m_pRangEnd->setFixedSize(40*g_xScale, 40*g_xScale);
                m_pRangEnd->move(event->pos().x()-m_pRangEnd->width()/2,event->pos().y()-m_pRangEnd->height()/2);
                m_pRangEnd->show();
                m_ptRangEnd = ui2real(event->pos());
                updateRang(QPoint(m_pRangStart->x() + m_pRangStart->width()/2,m_pRangStart->y() + m_pRangStart->height()/2),event->pos());
                update();
                m_MouseTimer.stop();
                setMouseTracking(false);
                m_pCtrlWidget->raise();
            }
            else //都存在时，允许拖动起点和终点
            {
                if (m_pRangStart && m_pRangStart->geometry().contains(event->pos()))
                {
                    m_MouseTimer.start(30);
                    m_bDragRangStart = true;
                }
                else if(m_pRangEnd && m_pRangEnd->geometry().contains(event->pos()))
                {
                    m_MouseTimer.start(30);
                    m_bDragRangEnd = true;
                }
            }
            break;
        }
        m_ptLastMouse = event->pos();
        m_inertiaTimer.stop();
        m_xMove = 0;
        m_yMove = 0;
        m_bDragging = true;
    }
    // 把事件传给父窗口处理
    if (parentWidget()) {
        QMouseEvent *newEvent = new QMouseEvent(event->type(), event->localPos(), event->windowPos(), event->screenPos(), event->button(), event->buttons(), event->modifiers(), event->source());
        QCoreApplication::postEvent(parentWidget(), newEvent);
    }
}

void SlamWidget::mouseMoveEvent(QMouseEvent *event)
{
    switch(m_Mode)
    {
    case MAP_ERROR:
    case MAP_MINSIZE:
        break;
    case MAP_NORMAL:
        break;
    case MAP_NAVIGATING:
        break;
    case  MAP_NAVIGATE:
        if(m_bDragging && m_pDest)
        {
            QPoint delta = event->pos() - m_ptLastMouse;
            m_fDestAngle = std::atan2(delta.y(), delta.x()) * 180 / M_PI + 90;
            QPixmap pixmap(":/images/map_dest.png");
            QTransform transform;
            transform.translate(m_pDest->width() / 2, m_pDest->height() / 2);
            transform.rotate(m_fDestAngle);
            transform.translate(-m_pDest->width() / 2, -m_pDest->height() / 2);
            QPixmap rotatedPixmap = pixmap.scaled(m_pDest->width(), m_pDest->height(), Qt::KeepAspectRatio, Qt::SmoothTransformation).transformed(transform, Qt::SmoothTransformation);
            m_pDest->setPixmap(rotatedPixmap);
            return;
        }
        break;
    case MAP_SETROBOT:
        if(m_bDragging)
        {
            QPoint delta = event->pos() - m_ptLastMouse;
            qreal angle = std::atan2(delta.y(), delta.x()) * 180 / M_PI + 90;
            setRobotRotation(angle);
            return;
        }
        break;
    case MAP_RANG:
        if(m_pRangStart && !m_pRangEnd)
        {
            updateRang(QPoint(m_pRangStart->x() + m_pRangStart->width()/2,m_pRangStart->y() + m_pRangStart->height()/2),
                       event->pos());
            update();
        }
        if(m_bDragRangStart)
        {
            m_pRangStart->move(event->pos().x()-m_pRangStart->width()/2,event->pos().y()-m_pRangStart->height()/2);
            m_ptRangStart = ui2real(event->pos());
            updateRang(QPoint(m_pRangStart->x() + m_pRangStart->width()/2,m_pRangStart->y() + m_pRangStart->height()/2),
                       QPoint(m_pRangEnd->x() + m_pRangEnd->width()/2,m_pRangEnd->y() + m_pRangEnd->height()/2));
            update();
            return;
        }
        else if(m_bDragRangEnd)
        {
            m_pRangEnd->move(event->pos().x()-m_pRangStart->width()/2,event->pos().y()-m_pRangStart->height()/2);
            m_ptRangEnd = ui2real(event->pos());
            updateRang(QPoint(m_pRangStart->x() + m_pRangStart->width()/2,m_pRangStart->y() + m_pRangStart->height()/2),
                       QPoint(m_pRangEnd->x() + m_pRangStart->width()/2,m_pRangEnd->y() + m_pRangEnd->height()/2));
            update();
            return;
        }
        break;
    }
    if(m_bDragging)
    {
        QPoint delta = event->pos() - m_ptLastMouse;
        setOffSet(m_ptOffset.x() + delta.x(),
                  m_ptOffset.y() +delta.y());
        m_xMove = delta.x();
        m_yMove = delta.y();
        m_ptLastMouse = event->pos();
        update();
    }
    // 把事件传给父窗口处理
    if (parentWidget()) {
        QMouseEvent *newEvent = new QMouseEvent(event->type(), event->localPos(), event->windowPos(), event->screenPos(), event->button(), event->buttons(), event->modifiers(), event->source());
        QCoreApplication::postEvent(parentWidget(), newEvent);
        return;
    }
}

void SlamWidget::mouseReleaseEvent(QMouseEvent *event)
{
    switch(m_Mode)
    {
    case MAP_ERROR:
    case MAP_MINSIZE:

        break;
    case MAP_NORMAL:
    case MAP_NAVIGATING:
        if(m_bDragging && event->button() == Qt::LeftButton)
        {
            m_inertiaTimer.start(30);
        }
        break;
    case  MAP_NAVIGATE:
        if(m_pDest)
        {
            if(!m_goDestBtn)
            {
                m_goDestBtn = new TouchButton(this);
                m_goDestBtn->setStyleSheet(
                    QString("QPushButton { "
                            "   color: white; "
                            "   font: %1px \"微软雅黑\";"
                            "   background-color:#007BFF; "
                            "   border: none; "
                            "   padding: 5px 15px; "
                            "   border-radius: 4px; "
                            "   margin-top: 10px; "
                            "} "
                            "QPushButton:pressed { "
                            "   background-color: #0056b3; "
                            "} ").arg(int(18*g_xScale)));
                m_goDestBtn->setText("开始");
                m_pCtrlWidget->raise();
                connect(m_goDestBtn,&QPushButton::clicked, this, [=](){
                    m_goDestBtn->hide();
                    if(m_pCurRobot)
                    {
                        m_bToDest = false;
                        m_pCurRobot->startNavigatione(m_ptDest, m_fDestAngle);
                        m_Mode = MAP_NAVIGATING;
                        m_MouseTimer.stop();
                    }
                });
            }
            m_goDestBtn->move(m_pDest->x() + m_pDest->width()+ 10, m_pDest->y() + m_pDest->height()/2- m_goDestBtn->height()/2);
            m_goDestBtn->show();
        }
        break;
    case MAP_SETROBOT:
        //计算方向
        {
            QPoint delta = event->pos() - m_ptLastMouse;
            qreal angle = std::atan2(delta.y(), delta.x()) * 180 / M_PI + 90;
            if(m_pCurRobot)
            {
                m_pCurRobot->initPosition(ui2real(m_ptLastMouse),angle);
            }
            m_pSetStartBtn->setChecked(false);
        }

        break;
    case MAP_RANG:
        if(m_bDragRangStart ||m_bDragRangEnd)
        {
            m_MouseTimer.stop();
        }
        else if(m_bDragging && event->button() == Qt::LeftButton)
        {
            m_inertiaTimer.start(30);
        }
        break;
    }

    m_bDragging = false;
    m_bDragRangStart = false;
    m_bDragRangEnd = false;
    // 把事件传给父窗口处理
    if (parentWidget()) {
        QMouseEvent *newEvent = new QMouseEvent(event->type(), event->localPos(), event->windowPos(), event->screenPos(), event->button(), event->buttons(), event->modifiers(), event->source());
        QCoreApplication::postEvent(parentWidget(), newEvent);
        return;
    }
}

void SlamWidget::paintEvent(QPaintEvent *event)
{
    Q_UNUSED(event);
    QPainter painter(this);
    painter.setRenderHint(QPainter::SmoothPixmapTransform);
    if(m_pCurRobot)
    {
        if(!m_pCurRobot->pImgMap()->isNull())
            painter.drawImage(m_ptOffset,m_pCurRobot->pImgMap()->scaled(m_pCurRobot->pImgMap()->size() * m_scaleFactor,Qt::KeepAspectRatio,Qt::SmoothTransformation));
        if(!m_pCurRobot->pImgPointCloud()->isNull())
            painter.drawImage(real2ui(m_pCurRobot->ptPointCloud()),m_pCurRobot->pImgPointCloud()->scaled(m_pCurRobot->pImgPointCloud()->size() * m_scaleFactor,Qt::KeepAspectRatio,Qt::SmoothTransformation));

        if(m_Mode == MAP_RANG)
        {
            if(m_pRangStart)
            {
                QPoint ptStart = QPoint(0,0);
                QPoint ptEnd = QPoint(0,0);
                ptStart = m_pRangStart->mapTo(this, m_pRangStart->rect().center());
                if(m_pRangEnd)
                {
                    ptEnd = m_pRangEnd->mapTo(this, m_pRangEnd->rect().center());
                    painter.setPen(QPen(Qt::red, 1));
                    painter.drawLine(ptStart, ptEnd);
                }
                else
                {
                    ptEnd = mapFromGlobal(QCursor::pos());
                    painter.setPen(QPen(Qt::red, 1));
                    painter.drawLine(ptStart, ptEnd);
                }
            }
        }
        else if(m_Mode == MAP_NAVIGATING || m_Mode == MAP_NAVIGATE)
        {
            if(m_pRobot && m_pDest)
            {
                QPoint ptStart = m_pRobot->mapTo(this, m_pRobot->rect().center());
                QPoint ptEnd = m_pDest->mapTo(this, m_pDest->rect().center());
                painter.setPen(QPen(Qt::green, 1));
                painter.drawLine(ptStart, ptEnd);
            }
        }
    }
}

void SlamWidget::wheelEvent(QWheelEvent *event)
{
    switch(m_Mode)
    {
    case MAP_ERROR:
    case MAP_MINSIZE:
        break;
    case MAP_NORMAL:
    case  MAP_NAVIGATE:
    case MAP_NAVIGATING:
    case MAP_SETROBOT:
    case MAP_RANG:
    {
        QPointF numDegrees = event->angleDelta() / 8;
        QPointF numSteps = numDegrees / 15;

        if (!numSteps.isNull()) {
            if (numSteps.y() > 0) {
                setScaleFactor(event->pos(),m_scaleFactor * 1.1);
            } else {
                setScaleFactor(event->pos(),m_scaleFactor * 0.9);
            }
        }
        update();
    }
    break;
    }
}


bool SlamWidget::event(QEvent *event)
{
    switch(event->type())
    {
    case QEvent::TouchBegin:
    case QEvent::TouchUpdate:
    {
        QTouchEvent *touchEvent = static_cast<QTouchEvent *>(event);
        QList<QTouchEvent::TouchPoint> touchPoints = touchEvent->touchPoints();
        if (touchPoints.count() >= 2)
        {
            const QTouchEvent::TouchPoint &touchPoint0 = touchPoints.first();
            const QTouchEvent::TouchPoint &touchPoint1 = touchPoints.last();
            qreal currentDistance = QLineF(touchPoint0.pos(), touchPoint1.pos()).length()
                                    / QLineF(touchPoint0.startPos(), touchPoint1.startPos()).length();
            if(m_last2TouchDistance>0)
            {
                switch(m_Mode)
                {
                case MAP_ERROR:
                case MAP_MINSIZE:
                    break;
                case MAP_NORMAL:
                case MAP_NAVIGATING:
                case MAP_NAVIGATE:
                case MAP_SETROBOT:
                case MAP_RANG:
                    setScaleFactor(QPoint((touchPoint0.pos().x() + touchPoint1.pos().x())/2,(touchPoint0.pos().y() + touchPoint1.pos().y())/2),
                              m_scaleFactor /m_last2TouchDistance *  currentDistance);

                break;
                }
            }
            m_last2TouchDistance = currentDistance;
        }
        else
        {
            m_last2TouchDistance = 0;
        }
        return true;
    }
    case QEvent::TouchEnd:
        m_last2TouchDistance = 0;
        return true;
    default:
        break;
    }
    return QWidget::event(event);
}

void SlamWidget::reSetScale()
{
    if(m_pCurRobot)
    {
        float fRes = m_pCurRobot->mapRes()/m_scaleFactor; //缩放后一像素代表多少米
        float arr[] = {float(0.1),float(0.2),float(0.5),1,2,5,10,20,50,100,200,500,1000,2000,5000,10000,20000,50000,100000};
        for(int i = 0; i < sizeof(arr)/sizeof(float); ++i)
        {
            if(arr[i]/fRes >= int(60*g_xScale))
            {
                m_pScale->setText(QString("%1m").arg(int(arr[i])));
                m_pScale->setFixedWidth(arr[i]/fRes+20);
                m_pScale->move(width() - m_pScale->width()-5, height()- m_pScale->height()-5);
                break;
            }
        }
    }
}

QPointF SlamWidget::ui2real(QPoint pt)
{
    if(m_pCurRobot)
    {
        return QPointF((pt.x() - m_ptOffset.x())/m_scaleFactor*m_pCurRobot->mapRes() - m_pCurRobot->mapOrigin().x(),
                       (m_ptOffset.y() - pt.y())/m_scaleFactor*m_pCurRobot->mapRes() - m_pCurRobot->mapOrigin().y());
    }
    return QPointF(0,0);
}

QPoint SlamWidget::real2ui(QPointF pt)
{
    if(m_pCurRobot)
    {
        return QPoint((pt.x() + m_pCurRobot->mapOrigin().x())/m_pCurRobot->mapRes()*m_scaleFactor + m_ptOffset.x(),
                      m_ptOffset.y() - (pt.y() + m_pCurRobot->mapOrigin().y())/m_pCurRobot->mapRes()*m_scaleFactor);
    }
    return QPoint(0,0);
}

void SlamWidget::bindRobot()
{
    if(m_pCurRobot)
    {
        connect(m_pCurRobot, &Robot::mapChanged,this, &SlamWidget::onMapChanged,Qt::UniqueConnection);
        connect(m_pCurRobot,&Robot::robotPositionChanged,this,[this](){
            if(m_Mode != MAP_SETROBOT)
            {
                setRobotPosition(m_pCurRobot->mapRobot());
                setRobotRotation(m_pCurRobot->mapRobotAngle());
                if(m_pCenterBtn->isChecked())
                {
                    setOffSet(width()/2 - (m_pCurRobot->mapRobot().x() + m_pCurRobot->mapOrigin().x()) / m_pCurRobot->mapRes() * m_scaleFactor,
                              height()/2 + (m_pCurRobot->mapRobot().y() + m_pCurRobot->mapOrigin().y()) / m_pCurRobot->mapRes() * m_scaleFactor);
                    m_inertiaTimer.stop();
                }
                if(m_Mode == MAP_NAVIGATING
                    && (m_pCurRobot->mapRobot() - m_ptDest).manhattanLength() < 1
                    && (std::abs((int)m_pCurRobot->mapRobotAngle()%360 - (int)m_fDestAngle%360)) < 5)
                {
                    m_bToDest = true;
                    m_pNavigateBtn->setChecked(false);
                    // Add your code here to handle the condition when the robot reaches the destination
                }
            }
        });
        connect(m_pCurRobot, &Robot::pointCloudChanged,this, [this](){update();});
    }
}

void SlamWidget::showEvent(QShowEvent *event)
{
    Q_UNUSED(event);
    if(m_pCurRobot)
    {
        m_pCurRobot->startPosition();
        m_pCurRobot->startPointCloud();
    }
}

void SlamWidget::hideEvent(QHideEvent *event)
{
    Q_UNUSED(event);
    if(m_pCurRobot)
    {
        m_pCurRobot->stopPosition();
        m_pCurRobot->stopPointCloud();
    }
    m_pNavigateBtn->setChecked(false);
    m_pSetStartBtn->setChecked(false);
    m_pRangBtn->setChecked(false);
}

void SlamWidget::setRobotPosition(QPointF pt)
{
    QPoint ptUI = real2ui(pt);
    m_pRobot->move(ptUI.x() - m_pRobot->width()/2 ,ptUI.y() - m_pRobot->height()/2);
}


void SlamWidget::setRobotRotation(float nAngle)
{
    //修改m_pRobot的角度为angle
    QPixmap pixmap(":/images/map_robot.png");
    QTransform transform;
    transform.translate(m_pRobot->width() / 2, m_pRobot->height() / 2);
    transform.rotate(nAngle);
    transform.translate(-m_pRobot->width() / 2, -m_pRobot->height() / 2);
    QPixmap rotatedPixmap = pixmap.scaled(m_pRobot->width(), m_pRobot->height(), Qt::KeepAspectRatio, Qt::SmoothTransformation).transformed(transform, Qt::SmoothTransformation);
    m_pRobot->setPixmap(rotatedPixmap);
}

void SlamWidget::setScaleFactor(QPoint ptCenter, double newScaleFactor)
{
    double oldScaleFactor = m_scaleFactor;
    m_scaleFactor = qBound(0.3, newScaleFactor, 3.0);
    if(m_pZoomSlider)
        m_pZoomSlider->setValue(newScaleFactor * 100);
    reSetScale();
    QPoint oldCenter = (ptCenter - m_ptOffset) / oldScaleFactor;
    QPoint newCenter = (ptCenter - m_ptOffset) / m_scaleFactor;
    setOffSet(m_ptOffset.x() + (newCenter.x() - oldCenter.x())* m_scaleFactor,
              m_ptOffset.y() + (newCenter.y() - oldCenter.y())* m_scaleFactor);
    resetCtrlPos();
    m_pCenterBtn->setChecked(false);
    update();
}

void SlamWidget::setOffSet(int x, int y)
{
    m_ptOffset = QPoint(x,y);
    if (m_ptOffset.x() < - m_pixMap.width()* m_scaleFactor+10)
        m_ptOffset.setX(-m_pixMap.width()* m_scaleFactor+10);

    if (m_ptOffset.y() < - m_pixMap.height()* m_scaleFactor+10)
        m_ptOffset.setY(-m_pixMap.height()* m_scaleFactor+10);

    if (m_ptOffset.x() > width() -10)
        m_ptOffset.setX(width() -10);

    if (m_ptOffset.y() > height() -10)
        m_ptOffset.setY(height() -10);
    resetCtrlPos();
    update();
    if(m_pCurRobot)
    {
        setRobotPosition(m_pCurRobot->mapRobot());
    }
}

void SlamWidget::updateRang(QPoint ptStart, QPoint ptEnd)
{
    if(m_pRangLab)
    {
        if(m_pCurRobot)
        {
            double distance = std::sqrt(std::pow(ptEnd.x() - ptStart.x(), 2) + std::pow(ptEnd.y() - ptStart.y(), 2));
            m_pRangLab->setText(QString::number(distance/m_scaleFactor *m_pCurRobot->mapRes() , 'f', 2) + "m");
            QFontMetrics metrics(m_pRangLab->font());
            int textWidth = metrics.horizontalAdvance(m_pRangLab->text()) + 10*g_xScale; // Add some padding
            m_pRangLab->setFixedWidth(textWidth);
            m_pRangLab->move(ptEnd.x() + 30*g_xScale, ptEnd.y() - m_pRangLab->height()/2);
            m_pRangLab->show();
        }
    }
}

void SlamWidget::resetCtrlPos()
{
    if (m_pRangStart) {
        QPoint ptRangStart =real2ui(m_ptRangStart);
        m_pRangStart->move(ptRangStart.x() - m_pRangStart->width()/2,ptRangStart.y() - m_pRangStart->height()/2);
    }
    if (m_pRangEnd){
        QPoint ptRangEnd =real2ui(m_ptRangEnd);
        m_pRangEnd->move(ptRangEnd.x() - m_pRangEnd->width()/2,ptRangEnd.y() - m_pRangEnd->height()/2);
        if(m_pRangLab)
        {
            m_pRangLab->move(ptRangEnd.x() +  m_pRangEnd->width()/2 +10,ptRangEnd.y() - m_pRangLab->height()/2);
        }
    }
    if(m_pDest)
    {
        QPoint ptDest =real2ui(m_ptDest);
        m_pDest->move(ptDest.x() - m_pDest->width()/2,ptDest.y() - m_pDest->height()/2);
        if(m_goDestBtn)
        {
            m_goDestBtn->move(m_pDest->x() + m_pDest->width()+ 10, m_pDest->y() + m_pDest->height()/2- m_goDestBtn->height()/2);
        }
    }
}

void SlamWidget::resizeEvent(QResizeEvent *event)
{
    Q_UNUSED(event);
    setOffSet((width() - m_pixMap.width() * m_scaleFactor)/2,(height() - m_pixMap.height()* m_scaleFactor)/2);
    m_pCtrlWidget->setGeometry(width() - 70*g_xScale,0,60*g_xScale,height() - 60 * g_xScale);
    m_pScale->move(width() - m_pScale->width()-5, height()- m_pScale->height()-5);
}

void SlamWidget::onInertiaMove()
{
    setOffSet(m_ptOffset.x() + m_xMove, m_ptOffset.y() + m_yMove);
    m_xMove *= 0.9;
    m_yMove *= 0.9;
    if (abs(m_xMove)+abs(m_yMove) < 1)
    {
        m_inertiaTimer.stop();
    }
    update();
}

void SlamWidget::onMouseTimer()
{
    if(!m_bDragging || m_bDragRangStart || m_bDragRangEnd)
    {
        QPoint cursorPos = mapFromGlobal(QCursor::pos());
        int margin = 20; // 边缘检测的距离

        if (cursorPos.x() < margin) {
            setOffSet(m_ptOffset.x() + 10, m_ptOffset.y());
        } else if (cursorPos.x() > width() - margin) {
            setOffSet(m_ptOffset.x() - 10, m_ptOffset.y());
        }

        if (cursorPos.y() < margin) {
            setOffSet(m_ptOffset.x(), m_ptOffset.y() + 10);
        } else if (cursorPos.y() > height() - margin) {
            setOffSet(m_ptOffset.x(), m_ptOffset.y() - 10);
        }
        if(m_pRangStart && !m_pRangBtn)
        {
            updateRang(QPoint(m_pRangStart->x() + m_pRangStart->width()/2,m_pRangStart->y() + m_pRangStart->height()/2),
                       cursorPos);
        }
    }
}


